#ifndef CUB_SPLINE_HPP
#define CUB_SPLINE_HPP
//=========================================================
#include "vector.hpp"
#include "matrix.hpp"
#include <stdio.h>
#include <math.h>
//=========================================================
//  POINT
//=========================================================
//объявление типа точка
struct point{
  double x;
  double y;
  double speed;
  point *next;
};
//---------------------------------------------------------
//добавление точки в конец списка
point *add(point *first,double x, double y,double speed){
  point *tmp1, *tmp2;
  tmp2 = new point;
  tmp2->next = NULL;
  tmp2->x = x;
  tmp2->y = y;
  tmp2->speed = speed;
  tmp1 = first;
  if(tmp1 != NULL){
    while(tmp1->next != NULL){
      tmp1 = tmp1->next;
    }
    tmp1->next = tmp2;
  }else{
    first = tmp2;
  }
  return first;
}
//=========================================================
//  CUBE_SPLINE
//=========================================================
//  n количество точек
//  a вектор набора точек
//  P заданные точки
//  Pp производные
//  Bt матрица коэффициентов
//=========================================================
point *cube_spline(int n, double **a, FILE *fi, FILE *flog){
  point *return_points = NULL;
  //-------------------------------------------------------
  //точки P
  vector *P = new vector[n];
  fprintf(flog,"--points P[i]:\n");
  for(int i=0; i<n; i++){
    P[i] = vector(3,a[i]);
    fprintf(flog,"%d speed=%g: ",i+1,a[i][2]);
    print(P[i],flog);
  }
  //-------------------------------------------------------
  //производные P'[0]...P'[n]
  vector *Pp = new vector[n];
  fprintf(flog,"--derivatives Pp[0] and Pp[n]:\n");
  for(int i=0; i<n; i++) 
    Pp[i] = vector(2);
  Pp[0] = P[1]-P[0]; //(P[1]-P[0])/(delta t)
  Pp[n-1] = P[n-1]-P[n-2];
  fprintf(flog,"1: ");
  print(Pp[0],flog);
  fprintf(flog,"%d: ",n);
  print(Pp[n-1],flog);
  fprintf(flog,"others will be found later\n");
  //-------------------------------------------------------
  //моменты T[i]
  vector T(n);
  T[0] = sqrt(pow(P[1][0]-P[0][0],2)+pow(P[1][1]-P[0][1],2));
  for(int i=1; i<n; i++)
    T[i] = sqrt(pow(P[i][0]-P[i-1][0],2)+pow(P[i][1]-P[i-1][1],2));   
  Pp[0] = Pp[0]*(1/T[0]);
  Pp[n-1] = Pp[n-1]*(1/T[n-1]);   
  fprintf(flog,"--moments T[i]:\n");
  for(int i=0; i<n; i++)
    fprintf(flog,"T[%d] = %g\n",i,T[i]);
  //-------------------------------------------------------
  //производные P'[1]..P'[n-1]
  fprintf(flog,"--finding rest derivatives:\n[M]*[Pp] = [Pr]\n");
  //-------------------------------------------------------
  //матрица коэффициентов T
  matrixes M(n);
  M[0][0] = M[n-1][n-1] = 1;
  for(int i=1; i<n-1; i++){
    M[i][i-1] = T[i+1];
    M[i][i] = 2*(T[i]+T[i+1]);
    M[i][i+1] = T[i];
  }
  fprintf(flog,"[M] =\n");
  print_matrix(M,flog);
  fprintf(flog,"[M]^-1 =\n");
  M = inv_matrix(M);
  print_matrix(M,flog);
  //-------------------------------------------------------
  //вектор результатов
  vector *Pr = new vector[n];
  Pr[0] = Pp[0];
  Pr[n-1] = Pp[n-1];
  for(int i=1; i<n-1; i++){
    vector tmp1 = (P[i+1]-P[i]);
    vector tmp2 = (P[i]-P[i-1]);
    tmp1 = (pow(T[i],2)*tmp1);
    tmp2 = (pow(T[i+1],2)*tmp2);
    double k = (3/(T[i]*T[i+1]));
    Pr[i] = tmp1 + tmp2;
    Pr[i] = k*Pr[i];    
  }
  fprintf(flog,"[Pr] = \n");
  for(int i=0;i<n;i++)
    print(Pr[i],flog);
  matrixes tmp_m(n), tmp_m_r(n);
  for(int i=0; i<2; i++)
    for(int j=0; j<n; j++)
      tmp_m[j][i] = Pr[j][i];
  tmp_m_r = M*tmp_m;
  for(int i=0; i<n; i++)
    for(int j=0; j<2; j++)
      Pp[i][j] = tmp_m_r[i][j];
  fprintf(flog,"derivatives Pp[i]:\n");
  for(int i=0; i<n; i++)
    print(Pp[i],flog);
  //-------------------------------------------------------
  //построение сегментов
  fprintf(flog,"--building spline segments:\n");
  vector Tt(4), Pc_x(4), Pc_y(4), pol(4);
  double t,p_x,p_y;
  matrixes B(4), Bt(4), tmp_b(4);
  //-------------------------------------------------------
  //сегменты
  for(int k=0; k<n-1; k++){
    fprintf(flog,"segment %d:\n", k+1);
    fprintf(flog,"  Bt[i][k]: \n");
    //-----------------------------------------------------
    //матрица коэффициентов полинома
    Bt[0][0] = Bt[1][1] = 1;
    Bt[2][0] = -3/(pow(T[k+1],2));
    Bt[2][1] = -2/(T[k+1]);
    Bt[2][2] = 3/(pow(T[k+1],2));
    Bt[2][3] = -1/(T[k+1]);
    Bt[3][0] = 2/(pow(T[k+1],3));
    Bt[3][1] = 1/(pow(T[k+1],2));
    Bt[3][2] = -2/(pow(T[k+1],3));
    Bt[3][3] = 1/(pow(T[k+1],2));
    print_matrix(Bt,flog);
    //-----------------------------------------------------
    //матрица точек и производных
    fprintf(flog,"  P[k]\n  Pp[k]\n  P[k+1]\n  Pp[k+1] :\n");
    for(int i=0; i<2; i++){
      tmp_b[0][i] = P[k][i];
      tmp_b[1][i] = Pp[k][i];
      tmp_b[2][i] = P[k+1][i];
      tmp_b[3][i] = Pp[k+1][i];
    }
    print_matrix(tmp_b,flog);
    B = Bt*tmp_b;
    fprintf(flog,"  B[i][%d]:\n",k+1);
    print_matrix(B,flog);
    for(int i=0; i<4; i++){
      Pc_x[i] = B[i][0];
      Pc_y[i] = B[i][1];
    }
    //fprintf(flog,"  polynom coefficient on the segment B =\n");
    //fprintf(flog,"  X: ");
    //print(Pc_x,flog);
    //fprintf(flog,"  Y: ");
    //print(Pc_y,flog);
    //значение полинома
    t = 0;
    //fprintf(flog,"  polynom values:");
    //fprintf(flog,"t[%d]=%g, t[%d]=%g\n",k,T[k],k+1,T[k+1]);
    //-----------------------------------------------------
    while(t <= T[k+1]){
      //заполенение вектора степеней t
      Tt[0] = 1;
      Tt[1] = t;
      Tt[2] = pow(t,2);
      Tt[3] = pow(t,3);
      p_x = p_y = 0;
      //print(Tt,flog);
      pol = Tt*Pc_x;
      for(int i=0; i<4; i++)
        p_x = p_x + pol[i];
      //print(pol,flog);
      pol = Tt*Pc_y;
      for(int i=0; i<4; i++)
        p_y = p_y + pol[i];
      //print(pol,flog);
      fprintf(fi,"%f     %f    %f\n",p_x,p_y,P[k][2]);
      t = t+0.05;
      return_points = add(return_points,p_x,p_y,P[k][2]);
    }
  }
  return return_points; 
}
//=========================================================
#endif